#!/usr/bin/env python3
# encoding: utf-8
# @Author: Assistant
# @Date: 2025/07/17
import cv2
import numpy as np
import rospy
import signal
import time
import math
import os
from sensor_msgs.msg import Image
from std_msgs.msg import String, Bool
from std_srvs.srv import Trigger, TriggerResponse
from cv_bridge import CvBridge
from ainex_sdk import misc, common
from ainex_example.color_common import Common

class PurpleObjectTracker(Common):
    def __init__(self, name):
        # 初始化头部位置
        self.head_pan_init = 500   # 左右舵机的初始值
        self.head_tilt_look_down = 280  # 低头位置
        
        # 初始化ROS节点
        rospy.init_node(name)
        self.name = name
        self.running = True
        self.active = False  # 默认不激活，等待外部启动
        
        super().__init__(name, self.head_pan_init, self.head_tilt_look_down)
        
        # 设置退出处理函数
        signal.signal(signal.SIGINT, self.shutdown)
        
        # Initialize CV Bridge
        self.bridge = CvBridge()
        
        # State variables - 多阶段执行
        self.current_image = None
        self.current_state = "aligning"  # aligning -> ready -> picking -> walking -> throwing -> done
        self.pick_executed = False
        self.walk_executed = False
        self.throw_executed = False
        self.task_completed = False
        
        # 从ROS参数服务器加载配置
        self.load_config()
        
        # 初始化运动参数
        self.init_movement_params()
        
        # 图像参数
        self.image_width = rospy.get_param('~image_width', 640)
        self.image_height = rospy.get_param('~image_height', 480)
        self.center_x = self.image_width // 2
        
        # 从配置加载位置判断参数
        self.load_position_params()
        
        # ROS通信接口
        self.setup_ros_interface()

        # 显示窗口控制
        self.update_display_config()
        
        # Subscribe to camera topic
        self.image_subscriber = rospy.Subscriber('/camera/image_raw', Image, self.image_callback)
        
        # 初始化机器人动作
        self.motion_manager.run_action('walk_ready')
        
        # 设置低头位置
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        time.sleep(2)  # 等待舵机到位
        
        common.loginfo('Purple object tracker initialized with continuous alignment')
        print("Throw task ready, waiting for activation...")

    def update_display_config(self):
        """从参数服务器更新显示配置"""
        display_ns = '/system/display'
        main_flag = rospy.get_param(f'{display_ns}/show_throw_window', None)
        if main_flag is None:
            main_flag = rospy.get_param(f'{display_ns}/show_main_window', True)
        self.display_main_window = bool(main_flag)

        mask_flag = rospy.get_param(f'{display_ns}/show_detection_masks', True)
        self.display_mask_window = self.display_main_window and bool(mask_flag)

    @staticmethod
    def _safe_destroy_window(window_name):
        try:
            cv2.destroyWindow(window_name)
        except cv2.error:
            pass
        except Exception:
            pass

    def setup_ros_interface(self):
        """设置ROS通信接口"""
        # 状态发布器
        self.status_pub = rospy.Publisher('/throw/status', String, queue_size=1)
        self.active_pub = rospy.Publisher('/throw/active', Bool, queue_size=1)
        self.progress_pub = rospy.Publisher('/throw/progress', String, queue_size=1)
        
        # 控制服务
        self.start_service = rospy.Service('/throw/start', Trigger, self.start_service_callback)
        self.stop_service = rospy.Service('/throw/stop', Trigger, self.stop_service_callback)
        self.reset_service = rospy.Service('/throw/reset', Trigger, self.reset_service_callback)
        
        # 控制命令订阅
        self.control_sub = rospy.Subscriber('/throw/control', String, self.control_callback)

    def start_service_callback(self, req):
        """启动服务回调"""
        self.active = True
        self.running = True
        self.task_completed = False
        self.pick_executed = False
        self.walk_executed = False
        self.throw_executed = False
        self.current_state = "aligning"
        
        # 启动时先向前走两步
        print("Starting throw task - Walking forward 2 steps first...")
        self.publish_status("initial_walk")
        try:
            self.gait_manager.set_step(
                self.dsp_param,      # DSP参数
                self.step_distance,  # x方向步长（前进）
                0,                   # y方向步长（不侧移）
                0,                   # yaw角度（不转向）
                self.walking_param,  # 步态参数
                self.arm_swap,       # 摆臂幅度
                2                    # 走两步
            )
            time.sleep(2)  # 等待走完
            print("Initial walk completed, starting purple object alignment...")
        except Exception as e:
            print(f"Error during initial walk: {e}")
            rospy.logerr(f"Initial walk error: {e}")
        
        self.publish_status("started")
        return TriggerResponse(success=True, message="Throw task started")

    def stop_service_callback(self, req):
        """停止服务回调"""
        self.active = False
        self.gait_manager.stop()
        self.publish_status("stopped")
        return TriggerResponse(success=True, message="Throw task stopped")

    def reset_service_callback(self, req):
        """重置服务回调"""
        self.active = False
        self.gait_manager.stop()
        self.current_state = "aligning"
        self.pick_executed = False
        self.walk_executed = False
        self.throw_executed = False
        self.task_completed = False
        self.publish_status("reset")
        return TriggerResponse(success=True, message="Throw task reset")

    def control_callback(self, msg):
        """控制命令回调"""
        command = msg.data.lower()
        if command == "start":
            self.active = True
            self.running = True
            self.task_completed = False
        elif command == "stop":
            self.active = False
            self.gait_manager.stop()
        elif command == "reset":
            self.active = False
            self.gait_manager.stop()
            self.current_state = "aligning"
            self.pick_executed = False
            self.walk_executed = False
            self.throw_executed = False
            self.task_completed = False

    def publish_status(self, status):
        """发布状态"""
        msg = String()
        msg.data = status
        self.status_pub.publish(msg)
        
        active_msg = Bool()
        active_msg.data = self.active
        self.active_pub.publish(active_msg)
        
        progress_msg = String()
        progress_msg.data = f"State: {self.current_state}, Pick: {self.pick_executed}, Walk: {self.walk_executed}, Throw: {self.throw_executed}, Completed: {self.task_completed}"
        self.progress_pub.publish(progress_msg)

    def load_config(self):
        """从ROS参数服务器加载配置"""
        try:
            # 首先尝试从外部配置文件加载颜色阈值（与competition_controller.py保持一致）
            color_config = rospy.get_param('/image_processing/color_config', {})
            use_external_config = color_config.get('use_external_config', False)
            external_config_file = color_config.get('external_config_file', '')
            config_section = color_config.get('config_section', 'Mono')
            
            # 默认配置（根据提供的lab配置文件中的正确值）
            self.purple_lower = np.array([0, 139, 60])   # 从lab配置文件获取的正确值
            self.purple_upper = np.array([115, 179, 113]) # 从lab配置文件获取的正确值
            
            if use_external_config and external_config_file and os.path.exists(external_config_file):
                try:
                    print(f"尝试从外部配置文件加载紫色阈值: {external_config_file}")
                    from ainex_sdk import common
                    external_config = common.get_yaml_data(external_config_file)
                    
                    if 'lab' in external_config and config_section in external_config['lab']:
                        lab_config = external_config['lab'][config_section]
                        
                        if 'purple' in lab_config:
                            color_data = lab_config['purple']
                            if 'min' in color_data and 'max' in color_data:
                                self.purple_lower = np.array(color_data['min'])
                                self.purple_upper = np.array(color_data['max'])
                                print(f"从外部配置文件加载紫色范围: {self.purple_lower} ~ {self.purple_upper}")
                            else:
                                print("外部配置文件中紫色配置格式不正确")
                        else:
                            print("外部配置文件中没有找到紫色配置")
                    else:
                        print(f"外部配置文件中没有找到lab.{config_section}配置段")
                except Exception as e:
                    print(f"读取外部配置文件失败: {e}")
            else:
                # 尝试从ROS参数服务器获取颜色配置
                purple_config = rospy.get_param('/image_processing/colors/purple', {})
                if purple_config:
                    self.purple_lower = np.array(purple_config.get('lower', self.purple_lower))
                    self.purple_upper = np.array(purple_config.get('upper', self.purple_upper))
                    print(f"从ROS参数加载紫色范围: {self.purple_lower} ~ {self.purple_upper}")
                else:
                    print(f"使用默认紫色范围: {self.purple_lower} ~ {self.purple_upper}")
            
            # 从参数服务器获取舵机配置
            servo_config = rospy.get_param('/servo', {})
            self.head_pan_init = servo_config.get('head_pan_init', 500)
            self.head_tilt_look_down = servo_config.get('head_tilt_look_down', 280)
            
            # print(f"配置加载成功 - 紫色范围: {self.purple_lower} ~ {self.purple_upper}")
            
        except Exception as e:
            print(f"配置加载失败，使用默认值: {e}")
            # 使用默认配置（与外部配置文件保持一致）
            self.purple_lower = np.array([0, 139, 60])
            self.purple_upper = np.array([115, 179, 113])
            self.head_pan_init = 500
            self.head_tilt_look_down = 280

    def load_position_params(self):
        """从配置加载位置判断参数"""
        try:
            # 从参数服务器获取投掷任务配置
            throw_config = rospy.get_param('/tasks/throw', {})
            position_ratios = throw_config.get('position_ratios', {})
            detection_config = throw_config.get('detection', {})
            
            # 加载位置比例参数
            target_y_ratio = position_ratios.get('target_y_ratio', 0.6)
            self.target_y = int(self.image_height * target_y_ratio)
            
            # 加载检测参数
            self.x_tolerance = detection_config.get('center_tolerance', 15)  # 水平容差
            self.y_tolerance = detection_config.get('y_tolerance', 20)       # 垂直容差
            self.min_area = detection_config.get('min_area', 500)

            # 从参数服务器获取运动参数
            movement_config = throw_config.get('movement', {})
            self.walk_forward_steps = movement_config.get('walk_forward_steps', 6)
            self.walk_yaw = movement_config.get('walk_yaw', 2)
            
            # print(f"位置参数加载成功 - 目标Y位置: {self.target_y} (比例: {target_y_ratio})")
            # print(f"容差: X±{self.x_tolerance}px, Y±{self.y_tolerance}px, 最小面积: {self.min_area}")
            # print(f"移动参数 - 前进步数: {self.walk_forward_steps}, 转向角: {self.walk_yaw}")
            
        except Exception as e:
            print(f"位置参数加载失败，使用默认值: {e}")
            # 使用默认配置
            self.target_y = int(self.image_height * 0.6)
            self.x_tolerance = 15
            self.y_tolerance = 20
            self.min_area = 500
            self.walk_forward_steps = 6
            self.walk_yaw = 2

    def init_movement_params(self):
        """初始化运动参数"""
        self.calib_config = common.get_yaml_data('/home/ubuntu/ros_ws/src/ainex_example/config/calib.yaml')
        
        # 获取步态参数
        self.walking_param = self.gait_manager.get_gait_param()
        self.walking_param['step_height'] = 0.02    # 步高
        self.walking_param['pelvis_offset'] = 5      # 盆骨偏移
        self.walking_param['hip_pitch_offset'] = 15  # 髋关节偏移
        self.walking_param['body_height'] = 0.035    # 身体高度
        self.walking_param['init_y_offset'] = 0.0    # 初始y偏移
        
        # 运动参数设置
        self.dsp_param = [400, 0.2, 0.025]  # DSP参数
        self.arm_swap = 30                   # 摆臂幅度
        self.step_distance = 0.008          # 前进步长
        self.side_step_distance = 0.008     # 侧移步长

    def image_callback(self, msg):
        """ROS图像回调函数"""
        try:
            self.current_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            rospy.logerr(f"Failed to convert image: {e}")

    def detect_purple_object(self, image):
        """检测紫色物体"""
        if image is None:
            return None, None
        
        # 转换为LAB色彩空间
        lab = cv2.cvtColor(image, cv2.COLOR_BGR2LAB)
        
        # 创建紫色掩码
        mask = cv2.inRange(lab, self.purple_lower, self.purple_upper)
        
        # 形态学操作去除噪声
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
        
        # 查找轮廓
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        if len(contours) > 0:
            # 找到最大的轮廓
            largest_contour = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(largest_contour)
            
            # 过滤掉太小的物体
            if area > self.min_area:
                # 计算物体中心
                M = cv2.moments(largest_contour)
                if M["m00"] != 0:
                    cx = int(M["m10"] / M["m00"])
                    cy = int(M["m01"] / M["m00"])
                    
                    return (cx, cy), mask
        
        return None, mask

    def control_robot_movement(self, object_center):
        """连续对齐控制"""
        if not self.active:  # 如果任务未激活，不执行移动
            return
            
        if object_center is None:
            print("No purple object detected, searching...")
            self.gait_manager.stop()
            self.publish_status("searching")
            return
        
        cx, cy = object_center
        
        # 计算偏差
        error_x = cx - self.center_x  # 水平偏差
        error_y = cy - self.target_y  # 垂直偏差
        
        print(f"Purple object at ({cx}, {cy}), errors: x={error_x:.0f}, y={error_y:.0f}, state={self.current_state}")
        
        if self.current_state == "aligning":
            self.continuous_alignment(error_x, error_y)
        elif self.current_state == "ready":
            self.execute_pick()
        elif self.current_state == "picking":
            # 等待拾取动作完成
            pass
        elif self.current_state == "walking":
            # 等待行走完成
            pass
        elif self.current_state == "throwing":
            # 等待投掷动作完成
            pass

    def continuous_alignment(self, error_x, error_y):
        """连续对齐控制"""
        # 检查是否已经对齐
        if abs(error_x) <= self.x_tolerance and abs(error_y) <= self.y_tolerance:
            print("Object aligned! Ready for throw action.")
            self.current_state = "ready"
            self.publish_status("aligned")
            return
        
        # 计算运动命令
        x_step = 0    # 前后移动
        y_step = 0    # 左右侧移
        yaw_output = 0  # 转向（用于微调）
        
        # 垂直方向调整（前后移动）
        if abs(error_y) > self.y_tolerance:
            if error_y < 0:  # 物体在目标上方，前进
                x_step = self.step_distance * 0.7
                print(f"物体太高，前进调整: {error_y:.0f}px")
            else:  # 物体在目标下方，后退
                x_step = -self.step_distance * 0.5
                print(f"物体太低，后退调整: {error_y:.0f}px")
        
        # 水平方向调整（左右侧移）
        if abs(error_x) > self.x_tolerance:
            if error_x < 0:  # 物体偏左，机器人向左侧移
                yaw_output += 2
                print(f"物体偏左，左移调整: {error_x:.0f}px")
            else:  # 物体偏右，机器人向右侧移
                yaw_output += -2
                print(f"物体偏右，右移调整: {error_x:.0f}px")
        
        # 如果偏差很小，只使用转向微调（更精确）
        if abs(error_x) <= self.x_tolerance * 2 and abs(error_y) <= self.y_tolerance:
            x_step = 0
            y_step = 0
            if abs(error_x) > 5:  # 使用转向进行精细调整
                yaw_output = -2 if error_x > 0 else 2
                print(f"精细转向调整: {yaw_output}")
        
        try:
            # 执行连续移动
            self.gait_manager.set_step(
                self.dsp_param,      # DSP参数
                x_step,              # x方向步长（前后）
                0,                   # y方向步长（左右侧移）
                int(yaw_output),     # yaw角度（微调）
                self.walking_param,  # 步态参数
                self.arm_swap,       # 摆臂幅度
                0                    # 连续移动
            )
            
            print(f"连续对齐: x={x_step:.3f}, y={y_step:.3f}, yaw={yaw_output}")
            self.publish_status("aligning")
            
        except Exception as e:
            print(f"Error during continuous alignment: {e}")
            rospy.logerr(f"Continuous alignment error: {e}")
            self.publish_status("error")

    def execute_pick(self):
        """第一阶段：执行拾取动作"""
        if self.pick_executed:
            return
            
        try:
            print("Executing pick action...")
            self.publish_status("executing_pick")
            self.current_state = "picking"
            
            # 停止步态管理器
            self.gait_manager.stop()
            time.sleep(0.5)
            
            # 禁用步态管理器，准备执行动作
            self.gait_manager.disable()
            
            # 执行拾取动作
            self.motion_manager.run_action('pick_cone')
            
            # 等待动作完成
            time.sleep(3)
            
            # 恢复准备状态
            self.motion_manager.run_action('walk_ready')
            
            self.pick_executed = True
            print("Pick action completed! Starting walk phase...")
            self.execute_walk()
            
        except Exception as e:
            print(f"Error during pick action: {e}")
            rospy.logerr(f"Pick action error: {e}")
            self.publish_status("error")

    def execute_walk(self):
        """第二阶段：向前走"""
        if self.walk_executed:
            return
            
        try:
            print("Walking forward")
            self.publish_status("walking_forward")
            self.current_state = "walking"
            
            # 向前走
            # self.gait_manager.set_step(
            #     self.dsp_param,      # DSP参数
            #     self.step_distance,  # x方向步长（前进）
            #     0,                   # y方向步长（不侧移）
            #     self.walk_yaw,       # yaw角度
            #     self.walking_param,  # 步态参数
            #     self.arm_swap,       # 摆臂幅度
            #     self.walk_forward_steps                    
            # )
            
            # # 等待行走完成
            # time.sleep(2)
            
            self.walk_executed = True
            print("Walk completed! Starting throw phase...")
            self.execute_throw_sandbag()
            
        except Exception as e:
            print(f"Error during walk: {e}")
            rospy.logerr(f"Walk error: {e}")
            self.publish_status("error")

    def execute_throw_sandbag(self):
        """第三阶段：执行投掷沙包动作"""
        if self.throw_executed:
            return
            
        try:
            print("Executing throw_sandbag action...")
            self.publish_status("executing_throw")
            self.current_state = "throwing"
            
            # 停止步态管理器
            self.gait_manager.stop()
            time.sleep(0.5)
            
            # 禁用步态管理器，准备执行动作
            self.gait_manager.disable()
            
            # 执行投掷沙包动作
            # self.motion_manager.run_action('throw')
            
            # 等待动作完成
            time.sleep(1)
            
            # 恢复准备状态，为后续任务准备
            self.motion_manager.run_action('walk_ready')
            
            self.throw_executed = True
            self.task_completed = True
            self.current_state = "done"
            self.active = False
            print("Throw sequence completed! (Pick -> Walk -> Throw)")
            self.publish_status("completed")
            common.loginfo('Throw sequence executed successfully: pick -> walk -> throw_sandbag')
            
        except Exception as e:
            print(f"Error during throw_sandbag action: {e}")
            rospy.logerr(f"Throw_sandbag action error: {e}")
            self.publish_status("error")

    def set_servo(self, pan, tilt):
        """设置舵机位置"""
        try:
            # 使用motion_manager控制舵机，时间200ms，舵机23为左右，舵机24为上下
            self.motion_manager.set_servos_position(200, [[23, pan], [24, tilt]])
        except Exception as e:
            rospy.logerr(f"Servo control error: {e}")

    def shutdown(self, signum, frame):
        """节点关闭回调函数"""
        with self.lock:
            self.motion_manager.run_action('stand')
            self.running = False 
            self.active = False
            self.publish_status("shutdown")
            common.loginfo('%s shutdown' % self.name)

    def run(self):
        """主循环函数"""
        print("Waiting for camera image...")
        
        # 等待图像数据
        while self.current_image is None and self.running:
            rospy.sleep(0.1)
        
        print("Camera ready, throw task standby...")
        self.publish_status("ready")
        
        # 窗口显示状态
        windows_created = False
        mask_window_created = False
        
        while self.running and not rospy.is_shutdown():
            try:
                # 支持运行时更新显示配置
                self.update_display_config()
                if not self.display_main_window and windows_created:
                    self._safe_destroy_window('Purple Object Tracker - Continuous Alignment - ACTIVE')
                    windows_created = False
                if (not self.display_mask_window or not self.display_main_window) and mask_window_created:
                    self._safe_destroy_window('Purple Mask - ACTIVE')
                    mask_window_created = False

                if self.current_image is not None and self.active:
                    # 检测紫色物体
                    object_center, mask = self.detect_purple_object(self.current_image)
                    
                    # 创建显示图像
                    display_image = self.current_image.copy()
                    
                    if object_center is not None:
                        cx, cy = object_center
                        # 在图像上绘制物体中心
                        cv2.circle(display_image, (cx, cy), 10, (0, 255, 0), -1)
                        
                        # 绘制目标中心点和容差区域
                        cv2.circle(display_image, (self.center_x, self.target_y), 5, (0, 0, 255), -1)
                        cv2.rectangle(display_image, 
                                    (self.center_x - self.x_tolerance, self.target_y - self.y_tolerance),
                                    (self.center_x + self.x_tolerance, self.target_y + self.y_tolerance),
                                    (255, 0, 0), 2)
                        
                        # 计算偏差
                        error_x = cx - self.center_x
                        error_y = cy - self.target_y
                        
                        # 显示状态信息
                        alignment_status = "ALIGNED" if (abs(error_x) <= self.x_tolerance and abs(error_y) <= self.y_tolerance) else "ALIGNING"
                        cv2.putText(display_image, alignment_status, (10, 30), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0) if alignment_status == "ALIGNED" else (0, 255, 255), 2)
                        cv2.putText(display_image, f"State: {self.current_state.upper()}", (10, 60), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        cv2.putText(display_image, "THROW - CONTINUOUS ALIGNMENT", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        
                        # 显示偏差信息
                        cv2.putText(display_image, f"Error X: {error_x:.0f}px (±{self.x_tolerance})", (10, 120), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0) if abs(error_x) <= self.x_tolerance else (0, 255, 255), 2)
                        cv2.putText(display_image, f"Error Y: {error_y:.0f}px (±{self.y_tolerance})", (10, 150), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0) if abs(error_y) <= self.y_tolerance else (0, 255, 255), 2)
                        
                        # 控制机器人移动
                        if not self.task_completed:
                            self.control_robot_movement(object_center)
                    else:
                        cv2.putText(display_image, "NO PURPLE OBJECT", (10, 30), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
                        cv2.putText(display_image, f"State: {self.current_state.upper()}", (10, 60), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        cv2.putText(display_image, "THROW - CONTINUOUS ALIGNMENT", (10, 90), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
                        if not self.task_completed:
                            self.gait_manager.stop()
                            self.publish_status("searching")
                    
                    # 显示任务状态
                    if self.task_completed:
                        cv2.putText(display_image, "TASK COMPLETED", (10, 180), 
                                  cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 255), 2)
                    
                    # 绘制图像中心线和目标线
                    cv2.line(display_image, (self.center_x, 0), (self.center_x, self.image_height), (128, 128, 128), 1)
                    cv2.line(display_image, (0, self.target_y), (self.image_width, self.target_y), (0, 255, 255), 1)
                    cv2.putText(display_image, "Target", (self.image_width - 80, self.target_y - 10), 
                              cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 255), 1)
                    
                    # 显示图像
                    if self.display_main_window:
                        cv2.imshow('Purple Object Tracker - Continuous Alignment - ACTIVE', display_image)
                        windows_created = True
                        if mask is not None and self.display_mask_window:
                            cv2.imshow('Purple Mask - ACTIVE', mask)
                            mask_window_created = True
                        elif mask_window_created and (mask is None or not self.display_mask_window):
                            self._safe_destroy_window('Purple Mask - ACTIVE')
                            mask_window_created = False

                        # 检查退出键
                        key = cv2.waitKey(30) & 0xFF
                        if key == 27:  # ESC键
                            break
                    else:
                        rospy.sleep(0.05)
                        
                elif not self.active and windows_created:
                    # 任务未激活时关闭窗口
                    self._safe_destroy_window('Purple Object Tracker - Continuous Alignment - ACTIVE')
                    if mask_window_created:
                        self._safe_destroy_window('Purple Mask - ACTIVE')
                        mask_window_created = False
                    windows_created = False
                    rospy.sleep(0.1)
                else:
                    rospy.sleep(0.1)
                    
            except KeyboardInterrupt:
                break
            except Exception as e:
                print(f"Error in main loop: {e}")
                rospy.sleep(0.1)
        
        # 退出前动作
        self.gait_manager.stop()
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        self.publish_status("stopped")
        if windows_created:
            self._safe_destroy_window('Purple Object Tracker - Continuous Alignment - ACTIVE')
        if mask_window_created:
            self._safe_destroy_window('Purple Mask - ACTIVE')
        rospy.signal_shutdown('shutdown')

if __name__ == '__main__':
    try:
        tracker = PurpleObjectTracker('purple_object_tracker')
        tracker.run()
    except Exception as e:
        print(f"Failed to start tracker: {e}")
        rospy.logerr(f"Tracker initialization error: {e}")
